#ifndef RVO_NODE__GAZEBO_H
#define RVO_NODE__GAZEBO_H

// 包含必要的头文件
#include "ros/ros.h"
#include "geometry_msgs/Twist.h"
#include "geometry_msgs/Pose.h"
#include "geometry_msgs/Point.h"
#include "geometry_msgs/TwistStamped.h"
#include "std_msgs/Header.h"
#include "gazebo_msgs/ModelStates.h"
#include "gazebo_msgs/WorldState.h"
#include <string>
#include "3d_library/nav_rvo.h"  // 包含自定义的3D库
#include "rvo_ros/SetGoals.h"
#include "gazebo_msgs/WorldState.h"
#include "gazebo_msgs/ModelStates.h"
#include <math.h>
#include <time.h>
// #include "UAVPosVel.h"
#include "../../../devel/include/rvo_ros/UAVPosVel.h"

typedef rvo_ros::UAVPosVel_<std::allocator<void>> UAVPosVel;


// 常量定义
const int num_max = 20; // 最大智能体数量

// 全局变量声明
int num_agent = 0; // 当前智能体数量
int copy_num_agent = 20; // 复制智能体数量
bool arrived = false; // 到达目标标志

// 函数声明
float vel_ratio(float vel, float lo, float hi); // 速度比例计算函数

// ROS 话题发布者
ros::Publisher rvo_node_pub;
std::vector<ros::Publisher> single_cmd_vel_flu_pub;
ros::Publisher single_cmd_vel_flu_pub_new;

// 话题消息
gazebo_msgs::WorldState msg_pub;
geometry_msgs::Twist my_msg;


// 目标点列表
std::vector<geometry_msgs::Point> rvo_goals;

// RVO 规划器对象指针
RVO::RVOPlanner* rvo;

// 运动模型类型
std::string motion_model = "default";

// 回调函数声明
void rvo_velCallback(const gazebo_msgs::ModelStates::ConstPtr& sub_msg); // 速度回调函数
void get_initial_pos(const gazebo_msgs::ModelStates::ConstPtr &model_msg);
void rvo_true_velCallback(const UAVPosVel::ConstPtr &sub_msg);

// 服务函数声明
bool set_goals(rvo_ros::SetGoals::Request &req, rvo_ros::SetGoals::Response &res); // 设置目标服务回调函数

// 初始化目标点函数声明
void rvo_goals_init(); // 初始化目标点函数

// 目标范围数组
float limit_goal[4]; // 目标范围数组

#endif // RVO_NODE__GAZEBO_H

